#ifndef __CODE1_MOTOR_H
#define __CODE1_MOTOR_H

#include "stm32f10x.h"
#include "Code0_CONF.h"
#include "Code0_PARAMETER.h"

#define INSTRUCTION_MODE	 	0
																											 
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////																									
typedef struct	
{
	unsigned char Buff[MOTOR_RX_BUFF_SIZE];
	int NowIndex;
	int EndIndex; 
} MotorRXStruct;												
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////																								
typedef enum
{
	HEAD1											= 0,
	HEAD2											= 1,
	ID_SYNCWRITE							= 2,
	TOTAL_LENGTH							= 3,
	INSTRUCTION_SYNCWRITE			= 4,
	FIRST_ADDRESS							= 5,
	SINGLE_DATA_LENGTH				= 6,
	PACKET_HEAD_LENGTH				= 7
} MotorDataPacketHeadDef;

typedef enum
{
	ID                	= 0,
	LOW_POSITION 				= 1,
	HIGH_POSITION 			= 2,
	LOW_SPEED						= 3,
	HIGH_SPEED					= 4,
	MOTOR_WRITE_LENGTH	= 5
} MotorDataWriteDef;

typedef enum
{
	READ_ID							= 0,			//or IMU  or  FSR  
	READ_LOW_POSITION		= 1,			// X axis  low_byte data
	READ_HIGH_POSITION	= 2,			// X axis hign_byte data
	READ_LOW_LOADING		= 3,			// Y axis  low_byte data
	READ_HIGH_LOADING 	= 4,		  // Y axis hign_byte data
	READ_VOLTAGE				= 5,			// Z axis  low_byte data
	READ_RESERVE        = 6,			// Z axis hign_byte data
	READ_CHECKSUM				= 7,			
	MOTOR_READ_LENGTH		= 8
} MotorDataReadDef;	
	
typedef enum
{
	TORQUE_ID                	= 0,
	TORQUE_ENABLED		 				= 1, 
	MOTOR_TORQUE_LENGTH				= 2
} MotorTorqueWriteDef;																
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
// Definition of Motor Data
typedef struct
{
	unsigned char PacketHead_UB[PACKET_HEAD_LENGTH];
	unsigned char Write_UB[MOTOR_NUM_UB][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_UB;					
				 
	unsigned char PacketHead_RL[PACKET_HEAD_LENGTH];
	unsigned char Write_RL[MOTOR_NUM_RL][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_RL;

	unsigned char PacketHead_LL[PACKET_HEAD_LENGTH];
	unsigned char Write_LL[MOTOR_NUM_LL][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_LL;
  
	unsigned short WtiteLength_UB;			
	unsigned short WtiteLength_RL;
	unsigned short WtiteLength_LL;					 

	unsigned char Checksum_PacketHead[BODYPART_NUM];	
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	unsigned char Read[MOTOR_READ_TOTAL_NUM][MOTOR_READ_LENGTH];	
	unsigned char Battery[BODYPART_NUM];	
	unsigned char BatteryCount[BODYPART_NUM];		
} MotorDataStruct;

typedef struct
{
	unsigned char PacketHead_UB[PACKET_HEAD_LENGTH];
	unsigned char Write_UB[MOTOR_NUM_UB][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_UB;					
				 
	unsigned char PacketHead_RL[PACKET_HEAD_LENGTH];
	unsigned char Write_RL[MOTOR_NUM_RL][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_RL;

	unsigned char PacketHead_LL[PACKET_HEAD_LENGTH];
	unsigned char Write_LL[MOTOR_NUM_LL][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_LL;					 

	unsigned char Checksum_PacketHead[BODYPART_NUM];	
  
	unsigned short WtiteLength_UB;			
	unsigned short WtiteLength_RL;
	unsigned short WtiteLength_LL;
} MotortTorqueStruct;

typedef struct
{								
	unsigned char *PacketHead[BODYPART_NUM];		
	unsigned char *Write[BODYPART_NUM];
	unsigned char *Checksum[BODYPART_NUM];			
	unsigned short *WtiteLength[BODYPART_NUM];
}MotorWriteAddressStruct;

typedef struct
{								
	unsigned char *PacketHead[BODYPART_NUM];		
	unsigned char *Write[BODYPART_NUM];
	unsigned char *Checksum[BODYPART_NUM];			
	unsigned short *WtiteLength[BODYPART_NUM];
}MotorTorqueAddressStruct;					
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

// Definition of Motor Schedule
typedef struct {
	Bool Enabled;
	int NowIndex;
	int EndIndex;
	unsigned char List[MOTOR_SCHEDULE_SIZE][MOTOR_SCHEDULE_LIST_LENGTH];
	unsigned int Delay_ExecutedCount; // Count of executing the function Update_MotorSchedule_to_MotorData()
	float Delay_Time;
	WorkStatus TorqueChanged;
	CapacityStatus Capacity;
} MotorScheduleStruct;
		 
// Definition of Head Schedule
typedef struct { 	
	int NowIndex;
	int EndIndex;
	unsigned char List[MOTOR_SCHEDULE_SIZE][MOTOR_SCHEDULE_LIST_LENGTH];			 
	unsigned int Delay_ExecutedCount; // Count of executing the function Update_MotorSchedule_to_MotorData()
	float Delay_Time;
} HeadScheduleStruct;

// Definition of Motor Schedule List 
typedef enum {
	INSERT_DELAY_COUNT	= 255,
	SET_STEP_TIME				= 253,
	INSERT_DELAY_TIME		= 249,

	SET_POSITION	  		= MOTOR_NUM,								// instruction byte < SET_POSITION	-> id = instruction byte
	SET_TORQUE	 				= MOTOR_MAX_ID + TORQUE_BASE		// instruction byte < SET_TORQUE		-> id = instruction byte - TORQUE_BASE
}	MotorScheduleInstruction;

//Definition of Delay Time
typedef enum {
	Low_ms		= 1,
	High_ms		= 2,
	Low_us		= 3,
	High_us		= 4,
} ScheduleDelayDef;

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////	
void Motor_Init(void);	 
void MotorRX_Init(void);	
void MotorAddress_Init(void);
void MotorData_Init(void);
void MotorSchedule_Init(void);				
void HeadSchedule_Init(void);

void WriteMotor(BodyPartStatus part);		
void WriteTorque(BodyPartStatus part);
void CalculateChecksum(void);

Bool ReadMotor(unsigned char id_index);
Bool CheckReceive(BodyPartStatus bodypart);

void MotorScheduleSwitch(WorkStatus status);
void Update_MotorSchedule_to_MotorData(void);
void Update_HeadSchedule_to_MotorData(void);

Bool Add_motor_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4);
void Add_head_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4);
Bool IDtoPartNo(unsigned char id, int *part, int *no);
int ID2Index(unsigned char id);
#endif /* __CODE1_MOTOR_H */
